#pragma once
#include "ctorquecontrol.h"
//#include "CQueryTimer.h"

#define DEFAULT_JM (30194.48/1000/1000000)
#define DEFAULT_RATIO 100


class CCurrentInnerCon :
	public CAction
{
public:
	CCurrentInnerCon(void);
	CCurrentInnerCon(CSeaAmp &seaAmp){Init(seaAmp);}
	~CCurrentInnerCon(void);

	const Error *EnableAction(void);


	bool Init(CSeaAmp &seaAmp){return CAction::Init(seaAmp, TORQUECONTROL_ID);}
	void DoAct();
	void SetKP( double p ) { Kp = p; }
	double GetKP( void ) { return Kp; }

	void SetKI( double i ) { Ki = i; }
	double GetKI( void ) { return Ki; }

	void SetKD( double d ) { Kd = d; }
	double GetKD( void ) { return Kd; }


	void SetJmotor( double Jm=DEFAULT_JM ) { Jmotor=Jm; }
	double GetJmotor() {return Jmotor;}

private:
	double Kp;
	double Ki;
	double Kd;

	double delta[3];
	double Jmotor;
	double TorqueForAcc;
	double TorqueForTrackLoad;
	double output;

	ofstream ToruqeData;

	double timeInterval;

};

